#!/usr/bin/env python

# Copyright (c) 2021 Computer Vision Center (CVC) at the Universitat Autonoma de
# Barcelona (UAB).
#
# This work is licensed under the terms of the MIT license.
# For a copy, see <https://opensource.org/licenses/MIT>.

"""Example script to generate traffic in the simulation"""
import functools
import glob
import os
import sys
import time

try:
    sys.path.append(glob.glob('../carla/dist/carla-*%d.%d-%s.egg' % (
        sys.version_info.major,
        sys.version_info.minor,
        'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0])
except IndexError:
    pass

import carla
from pyproj import Proj
import math
from carla import VehicleLightState as vls

import argparse
import logging

import asyncio
import json
import uuid
import random
from websocketServer import WebSocketServer



def get_actor_blueprints(world, filter, generation):
    bps = []
    for i in filter:
        bp = world.get_blueprint_library().find(i)
        bps.append(bp)

    # bps = world.get_blueprint_library().filter(filter)

    if generation.lower() == "all":
        return bps

    # If the filter returns only one bp, we assume that this one needed
    # and therefore, we ignore the generation
    if len(bps) == 1:
        return bps

    try:
        int_generation = int(generation)
        # Check if generation is in available generations
        if int_generation in [1, 2]:
            bps = [x for x in bps if int(x.get_attribute('generation')) == int_generation]
            return bps
        else:
            print("   Warning! Actor Generation is not valid. No actor will be spawned.")
            return []
    except:
        print("   Warning! Actor Generation is not valid. No actor will be spawned.")
        return []



def main():
    ws_server = WebSocketServer(8083)
    ws_server.start()
    loop = asyncio.get_event_loop()

    argparser = argparse.ArgumentParser(
        description=__doc__)
    argparser.add_argument(
        '-n', '--number-of-vehicles',
        metavar='N',
        default=20,
        type=int,
        help='Number of vehicles (default: 30)')
    argparser.add_argument(
        '--filterv',
        metavar='PATTERN',
        default='vehicle.*',
        help='Filter vehicle model (default: "vehicle.*")')
    argparser.add_argument(
        '--generationv',
        metavar='G',
        default='All',
        help='restrict to certain vehicle generation (values: "1","2","All" - default: "All")')
    argparser.add_argument(
        '--asynch',
        action='store_true',
        help='Activate asynchronous mode execution')
    argparser.add_argument(
        '--car-lights-on',
        action='store_true',
        default=False,
        help='Enable automatic car light management')
    argparser.add_argument(
        '--hero',
        action='store_true',
        default=False,
        help='Set one of the vehicles as hero')


    args = argparser.parse_args()


    logging.basicConfig(format='%(levelname)s: %(message)s', level=logging.INFO)

    vehicles_list = []
    client = carla.Client("localhost", 2000)
    client.set_timeout(20.0)
    synchronous_master = False

    try:
        world = client.get_world()
        # world = client.load_world('SDWR0525')

        traffic_manager = client.get_trafficmanager(8000)
        # TM里的每一辆车至少保持2米的安全距离
        traffic_manager.set_global_distance_to_leading_vehicle(2.0)
        # TM里每一辆车都是默认速度的40%（默认所有车辆限速30km/h）
        traffic_manager.global_percentage_speed_difference(70.0)
        if True:
            settings = world.get_settings()
            traffic_manager.set_synchronous_mode(True)
            if not settings.synchronous_mode:
                print("开启同步模式")
                synchronous_master = True
                settings.synchronous_mode = True
                settings.fixed_delta_seconds = 0.1
                world.apply_settings(settings)
            else:
                synchronous_master = False
                settings.fixed_delta_seconds = 0.1
                world.apply_settings(settings)
        else:
            print("You are currently in asynchronous mode. If this is a traffic simulation, \
            you could experience some issues. If it's not working correctly, switch to synchronous \
            mode by using traffic_manager.set_synchronous_mode(True)")

        # 控制世界的天气和时间（太阳的位置）wwwwww
        weather = carla.WeatherParameters(
            cloudiness=0.0,  # 0-100  0是晴朗的天空，100是完全阴天
            precipitation=0.0,  # 0表示没有下雨，100表示大雨
            precipitation_deposits=0.0,  # 0表示道路上没有水坑，100表示道路完全被雨水覆盖
            wind_intensity=0.0,  # 0表示平静，100表示强风，风会影响雨向和树叶
            sun_azimuth_angle=270.0,  # 太阳方位角，0～360
            sun_altitude_angle=13.0,  # 太阳高度角，90是中午，-90是午夜
            fog_density=0.0,  # 0～100表示雾的浓度或厚度，仅影响RGB相机传感器
            fog_distance=0.0,  # 雾开始的距离，单位为米
            wetness=0.0,  # 0～100表示道路湿度百分比，仅影响RGB相机传感器
            fog_falloff=0.0,  # 雾的密度，0至无穷大，0表示雾比空气轻，覆盖整个场景，1表示与空气一样，覆盖正常大小的建筑物
            scattering_intensity=0.0,  # 控制光线对雾的穿透程度
            mie_scattering_scale=0.0,  # 控制光线与花粉或空气等大颗粒的相互作用，导致天气朦胧，光源周围有光晕，0表示无影响
            rayleigh_scattering_scale=0.0331,  # 控制光与空气分子等小粒子的相互作用，取决于光波长，导致白天蓝天或晚上红天
        )
        world.set_weather(weather)

        filterv = ['vehicle.dodge.charger_2020', 'vehicle.volkswagen.t2_2021', 'vehicle.ford.ambulance']
        F = [0.6, 0.2, 0.2]
        # 'vehicle.dodge.charger_2020', 'vehicle.tesla.model3'
        blueprints = get_actor_blueprints(world, filterv, args.generationv)
        # select 4 wheels vehicle
        # blueprints = [x for x in blueprints if int(x.get_attribute('number_of_wheels')) == 4]

        # blueprints = sorted(blueprints, key=lambda bp: bp.id)

        spawn_points = world.get_map().get_spawn_points()

        number_of_spawn_points = len(spawn_points)

        if args.number_of_vehicles < number_of_spawn_points:
            random.shuffle(spawn_points)
        elif args.number_of_vehicles > number_of_spawn_points:
            msg = 'requested %d vehicles, but could only find %d spawn points'
            logging.warning(msg, args.number_of_vehicles, number_of_spawn_points)
            args.number_of_vehicles = number_of_spawn_points

        # @todo cannot import these directly.
        SpawnActor = carla.command.SpawnActor
        SetAutopilot = carla.command.SetAutopilot
        FutureActor = carla.command.FutureActor

        # --------------
        # Spawn vehicles
        # --------------
        batch = []
        hero = args.hero
        for n, transform in enumerate(spawn_points):

            if n >= args.number_of_vehicles:
                break
            # blueprint = random.choice(blueprints)
            blueprint_list = random.choices(blueprints, weights=F, k=1)
            blueprint = blueprint_list[0]
            # print(type(blueprints), ", ", blueprint)
            if blueprint.has_attribute('color'):
                color = random.choice(blueprint.get_attribute('color').recommended_values)
                blueprint.set_attribute('color', color)
            if blueprint.has_attribute('driver_id'):
                driver_id = random.choice(blueprint.get_attribute('driver_id').recommended_values)
                blueprint.set_attribute('driver_id', driver_id)
            if hero:
                blueprint.set_attribute('role_name', 'hero')
                hero = False
            else:
                blueprint.set_attribute('role_name', 'autopilot')

            # spawn the cars and set their autopilot and light state all together
            batch.append(SpawnActor(blueprint, transform)
                .then(SetAutopilot(FutureActor, True, traffic_manager.get_port())))

        for response in client.apply_batch_sync(batch, synchronous_master):
            if response.error:
                logging.error(response.error)
            else:
                vehicles_list.append(response.actor_id)

        # Set automatic vehicle lights update if specified
        if args.car_lights_on:
            all_vehicle_actors = world.get_actors(vehicles_list)
            for actor in all_vehicle_actors:
                traffic_manager.update_vehicle_lights(actor, True)

        print('spawned %d vehicles, press Ctrl+C to exit.' % len(vehicles_list))

        SD_map = world.get_map()
        rule = Proj("+proj=tmerc +lon_0=121.2092870660126 +lat_0=31.292829882838856 +ellps=WGS84")


        while True:
            if not args.asynch and synchronous_master:
                world.tick()
                car_list = list()
                all_vehicle_actors = world.get_actors(vehicles_list)
                if len(all_vehicle_actors) > 0:
                    for vehicle in all_vehicle_actors:
                        # vehicle_alive = vehicle.is_alive
                        vehicle_id = vehicle.id
                        vehicle_type = vehicle.type_id
                        x = vehicle.get_transform().location.x
                        y = vehicle.get_transform().location.y
                        vehicle_angle = 90 - vehicle.get_transform().rotation.yaw
                        v = vehicle.get_velocity()
                        vehicle_speed = 3.6 * math.sqrt(v.x ** 2 + v.y ** 2 + v.z ** 2)
                        waypoint = SD_map.get_waypoint(vehicle.get_location())
                        lane_id = waypoint.lane_id
                        longitude, latitude = rule(x, -y, inverse=True)
                        if x == 0 and y == 0:
                            vehicle.destroy()
                            vehicles_list.remove(vehicle_id)
                        else:
                            car = {"id": vehicle_id, "ty": vehicle_type, "lo": longitude, "la": latitude,
                                   "ag": vehicle_angle, "speed": vehicle_speed, "laneNum": lane_id}
                            car_list.append(car)
                if len(car_list) > 0:

                    result = {"participants": car_list, "participantNum": len(car_list),
                              "timeStamp": str(time.time()), "globalTimeStamp": str(time.time())}
                    # print(result)
                    loop.run_until_complete(ws_server.broadcast(str(result).replace("'", '"')))

            else:
                world.wait_for_tick()

            time.sleep(0.03)

    finally:

        if not args.asynch and synchronous_master:
            settings = world.get_settings()
            settings.synchronous_mode = False
            settings.no_rendering_mode = False
            settings.fixed_delta_seconds = None
            world.apply_settings(settings)

        print('\ndestroying %d vehicles' % len(vehicles_list))
        client.apply_batch([carla.command.DestroyActor(x) for x in vehicles_list])

        time.sleep(0.5)

if __name__ == '__main__':

    try:
        main()
    except KeyboardInterrupt:
        pass
    finally:
        print('\ndone.')
